package edu.nus.cs4243.recon.logic;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import Jama.Matrix;
import edu.nus.cs4243.recon.model.Feature;
import edu.nus.cs4243.recon.model.HasCoordinates2d;
import edu.nus.cs4243.recon.model.Point3d;
import edu.nus.cs4243.recon.model.SceneView;
import edu.nus.cs4243.recon.model.models.Model;

/**
 * Reconstructs models from two views.
 * 
 * @author johannes
 */
public class TwoViewReconstructor {

	private TwoViewReconstructor() {
	}

	/**
	 * Reconstructs models from pair of scene views.
	 * 
	 * @param camera
	 *            the camera used to capture both views
	 * @param view1
	 *            the first view
	 * @param view2
	 *            the second view
	 * @param allModels
	 *            if set to true, all generated models will be returned.
	 * @return a list of possible models matching the two views
	 */
	public static List<Model> getPossibleModels(Camera camera, SceneView view1, SceneView view2,
			boolean allModels) {
		final EssentialMatrixEstimator essEst = new EssentialMatrixEstimator(view1, view2, camera);
		final RotationTranslationEstimator rotTransEst = new RotationTranslationEstimator(
				essEst.getRefinedEssentialMatrix());
		final Collection<Feature> commonFeatures = view1.getCommonFeatures(view2);
		final Matrix r1 = rotTransEst.getR1();
		final Matrix r2 = rotTransEst.getR2();
		Matrix t = rotTransEst.getT();

		List<Model> candidateModels = new ArrayList<Model>(4);
		List<Integer> noInvPts = new ArrayList<Integer>(4);
		try {
			noInvPts.add(reconstructModel(camera, candidateModels, commonFeatures, t, r1, view1,
					view2));
		} catch (RuntimeException e) {
		}
		try {
			noInvPts.add(reconstructModel(camera, candidateModels, commonFeatures, t, r2, view1,
					view2));
		} catch (RuntimeException e) {
		}
		t = t.times(-1);
		try {
			noInvPts.add(reconstructModel(camera, candidateModels, commonFeatures, t, r1, view1,
					view2));
		} catch (RuntimeException e) {
		}
		try {
			noInvPts.add(reconstructModel(camera, candidateModels, commonFeatures, t, r2, view1,
					view2));
		} catch (RuntimeException e) {
		}

		// TODO check whether these two models are really the ones we want!

		if (allModels)
			return candidateModels;
		
		// the maximum is the model that is behind both cameras
		List<Model> resultList = new ArrayList<Model>(2);
		int idx = getMaxIndex(noInvPts);
		resultList.add(candidateModels.remove(idx));
		noInvPts.remove(idx);

		// the minimum is the model that is in front of both cameras
		idx = getMinIndex(noInvPts);
		resultList.add(candidateModels.remove(idx));
		noInvPts.remove(idx);

		return resultList;
	}

	private static int getMaxIndex(List<Integer> l) {
		int max = l.get(0);
		int maxIndex = 0;
		for (int i = 1; i < l.size(); i++) {
			if (l.get(i) > max) {
				max = l.get(i);
				maxIndex = i;
			}
		}
		return maxIndex;
	}

	private static int getMinIndex(List<Integer> l) {
		int min = l.get(0);
		int minIndex = 0;
		for (int i = 1; i < l.size(); i++) {
			if (l.get(i) < min) {
				min = l.get(i);
				minIndex = i;
			}
		}
		return minIndex;
	}

	/**
	 * Create a Model using triangulation.
	 * 
	 * @param candidateModels
	 *            the model will be added to this list
	 * @param commonFeatures
	 *            set of all features that should be triangulated
	 * @param t
	 *            location of the second view
	 * @param r
	 *            rotation of the second view
	 * @param i1
	 *            first view
	 * @param i2
	 *            second view
	 * @return the number of points that lie behind the camera
	 */
	private static int reconstructModel(Camera camera, List<Model> candidateModels,
			Collection<Feature> commonFeatures, Matrix t, Matrix r, SceneView i1, SceneView i2) {
		Model m = new Model();
		Matrix c = r.transpose().times(t).times(-1);
		Matrix zeroTrans = new Matrix(3, 1);
		Matrix identityRot = Matrix.identity(3, 3);
		final SceneView left = i1;
		final SceneView right = i2;
		int result = 0;

		for (Feature f : commonFeatures) {
			final HasCoordinates2d fLeft = left.getLocationInView(f);
			final HasCoordinates2d fRight = right.getLocationInView(f);
			Matrix x = Triangulator.triangulate(camera, r, c, fLeft, fRight);

			m.addPoint(f, new Point3d(x.get(0, 0), x.get(1, 0), x.get(2, 0)));
			if (camera.isPointBehindCamera(x, t, r))
				result++;
			if (camera.isPointBehindCamera(x, zeroTrans, identityRot))
				result++;
		}
		candidateModels.add(m);
		return result;
	}

}
